/**
* Class offering access to some of the DS-GPM arduino GPS module functionnalities.
* @author: Jeremy Nicola
*/


#include "WProgram.h"
#include "GPS.h"
#include "/usr/share/arduino/libraries/Wire/Wire.h"
GPS::GPS()
{

}

GPS::GPS(int address)
{
 _address=address;
 //delay(3000); // IMPORTANT : no 3sec delay will cause parts of your soft not working properly ! ( issue experienced ! )
}

/**
* Adapted from the DS-GPM Arduino AppNote
*/
int GPS::GetDouble(int cmd){  // Get double register value from GPM

  int Value = 0; 
  byte H_Byte = 0;
  byte L_Byte = 0;

  Wire.beginTransmission(_address);
  Wire.send(cmd);				      // Send register start address
  Wire.endTransmission();

  Wire.requestFrom(_address, 2);			      // Request double register
  while(Wire.available() < 2);			      // Wait for double byte
  H_Byte = Wire.receive();                            // Store one
  L_Byte = Wire.receive();                            // Store two

  Value = (H_Byte * 10) + L_Byte;                     // Adjust for one byte

  return(Value);	                          
}

/**
* Adapted from the DS-GPM Arduino AppNote
*/
int GPS::GetSingle(int cmd)
{
 int Value = 0; 

  Wire.beginTransmission(_address);
  Wire.send(cmd);    // Send register start address
  Wire.endTransmission();
  Wire.requestFrom(_address, 1);     // Request register
  while(Wire.available() < 1);      // Wait for single byte
  Value = Wire.receive();          // and store

  return(Value);
}

Vector2D GPS::heading()
{
 _headHun=GetSingle(44);
 _headTen=GetSingle(45);
 _headUn=GetSingle(46);
 _headTenths=GetSingle(47);
 _headingDeg=(float)((int)_headHun*100+(int)_headTen*10+(int)_headUn+(int)_headTenths/10.);
 _headingVect._y=cos(radians(_headingDeg));
 _headingVect._x=sin(radians(_headingDeg));
 return _headingVect;
}

int GPS::hour()
{
 _hourTen=GetSingle(0);
 _hourUn=GetSingle(1);
 _hour=_hourTen*10+_hourUn;
 return _hour;
}

int GPS::minute()
{
 _minuteTen=GetSingle(2);
 _minuteUn=GetSingle(3);
 _minute=_minuteTen*10+_minuteUn;
 return _minute;
}

int GPS::second()
{
 _secondTen=GetSingle(4);
 _secondUn=GetSingle(5);
 _second=_secondTen*10+_secondUn;
 return _second;
}

float GPS::speed()
{
 _speedHun=GetSingle(52);
 _speedTen=GetSingle(53);
 _speedUn=GetSingle(54);
 _speedTenths=GetSingle(55);
 _GPSSpeed=_speedHun*100+_speedTen*10+_speedUn+_speedTenths/10.;
 return _GPSSpeed;
}

/**
* Returns the latitude value as an angle in radian comprised in [-pi/2;pi/2]
*/
float GPS::latitude()
{
 _latTens=GetSingle(14);
 _latUn=GetSingle(15);
 _latMinTens=GetSingle(16);
 _latMinUn=GetSingle(17);
 _latMinTenths=GetSingle(18);
 _latMinHun=GetSingle(19);
 _latMinThous=GetSingle(20);
 _latMinTenThous=GetSingle(21);
 _latChar=GetSingle(22);
 _latitudeDeg=_latTens*10.+_latUn+(_latMinTens*10.+_latMinUn+_latMinTenths/10.+_latMinHun/100.+_latMinThous/1000.+_latMinTenThous/10000.)/60.;
 _latitudeRad=radians(_latitudeDeg); 

 if(_latChar=='S')
 {
  _latitudeRad=-_latitudeRad;
  _latitudeDeg=-_latitudeDeg;
 }
  return _latitudeRad;
}

/**
* Returns the longitude value as an angle in radian comprised in [-pi;pi]
*/
float GPS::longitude()
{
 _longHun=GetSingle(23);
 _longTens=GetSingle(24);
 _longUn=GetSingle(25);
 _longMinTens=GetSingle(26);
 _longMinUn=GetSingle(27);
 _longMinTenths=GetSingle(28);
 _longMinHun=GetSingle(29);
 _longMinThous=GetSingle(30);
 _longMinTenThous=GetSingle(31);
 _longChar=GetSingle(32);
 _longitudeDeg=(_longHun*100+_longTens*10+_longUn+(_longMinTens*10+_longMinUn+_longMinTenths/10.+_longMinHun/100.+_longMinThous/1000.+_longMinTenThous/10000.)/60.);
 _longitudeRad=radians(_longitudeDeg);
 
 if(_longChar=='W')
 {
  _longitudeRad=-_longitudeRad;
  _longitudeDeg=-_longitudeDeg;
 }
 return _longitudeRad;
}

